import numpy as np
import math

class CalProof:
    def __init__(self):
        self.RR=np.zeros([3,4])
        self.MM=np.zeros([3,4])
        self.Settings=self.getsettings()    # 从文件读取到的可信度参数

    # 从文件中得到可信度参数
    def getsettings(self):
        # 读文件 建立参数表
        file_settings=open('data/settings.txt')
        files=file_settings.readlines()
        return [files[i:i+17] for i in range(0,204,17)]

    # 字符串读数
    def find_num(self, word):
        temp = word.split('=')
        return float(temp[1])

    # 计算传感器水平距离基本可信数
    def GetHDPara_R(self,section,HD):
        # 计算传感器水平距离基本可信数
        # 变量赋值
        zo = self.find_num(self.Settings[section][1])

        js = self.find_num(self.Settings[section][2])
        zj = self.find_num(self.Settings[section][3])
        ks = self.find_num(self.Settings[section][4])
        zb = self.find_num(self.Settings[section][5])

        o = self.find_num(self.Settings[section][6])

        azo =self.find_num(self.Settings[section][7])
        bzo = self.find_num(self.Settings[section][8])

        ajs = self.find_num(self.Settings[section][9])
        bjs = self.find_num(self.Settings[section][10])

        azj = self.find_num(self.Settings[section][11])
        bzj = self.find_num(self.Settings[section][12])

        aks = self.find_num(self.Settings[section][13])
        bks = self.find_num(self.Settings[section][14])

        azb = self.find_num(self.Settings[section][15])
        bzb = self.find_num(self.Settings[section][16])

        self.RR[0][3] = o

        # 分段函数的判断
        if ((HD >= 0) and (HD < zo)):
            self.RR[0][1] = azo
            self.RR[0][2] = bzo
        elif ((HD >= zo) and (HD < js)):
            self.RR[0][1] = (ajs - azo) * (HD - zo) / (js - zo) + azo
            self.RR[0][2] = (bjs - bzo) * (HD - zo) / (js - zo) + bzo
        elif ((HD >= js) and (HD < zj)):
            self.RR[0][1] = (azj - ajs) * (HD - js) / (zj - js) + ajs
            self.RR[0][2] = (bzj - bjs) * (HD - js) / (zj - js) + bjs
        elif ((HD >= zj) and (HD < ks)):
            self.RR[0][1] = (aks - azj) * (HD - zj) / (ks - zj) + azj
            self.RR[0][2] = (bks - bzj) * (HD - zj) / (ks - zj) + bzj
        elif (HD >= ks):
            self.RR[0][1] = azb
            self.RR[0][2] = bzb

    # 计算传感器方位夹角的基本可信数
    def GetAAPara_R(self, section, AA, Para=True):
        up=0.
        low=0.

        if (Para == True):   # 判断是否为空对地
            up =math.pi
            low = 0.
        else:
            up = math.pi / 2
            low = 0 - math.pi / 2

            # 变量赋值
        zo = self.find_num(self.Settings[section][1]) * math.pi / 180
        js = self.find_num(self.Settings[section][2]) * math.pi / 180
        zj = self.find_num(self.Settings[section][3]) * math.pi / 180
        ks = self.find_num(self.Settings[section][4]) * math.pi / 180
        zb = self.find_num(self.Settings[section][5]) * math.pi / 180

        o = self.find_num(self.Settings[section][6])

        azo = self.find_num(self.Settings[section][7])
        bzo = self.find_num(self.Settings[section][8])

        ajs = self.find_num(self.Settings[section][9])
        bjs = self.find_num(self.Settings[section][10])

        azj = self.find_num(self.Settings[section][11])
        bzj = self.find_num(self.Settings[section][12])

        aks = self.find_num(self.Settings[section][13])
        bks = self.find_num(self.Settings[section][14])

        azb = self.find_num(self.Settings[section][15])
        bzb = self.find_num(self.Settings[section][16])

        self.RR[1][3] = o

        if (AA < 0):
            AA = 0 - AA

        if ((AA >= low) and (AA < zo)):
            self.RR[1][1] = azo
            self.RR[1][2] = bzo

        elif ((AA >= zo) and (AA < js)):
            self.RR[1][1] = (ajs - azo) * (AA - zo) / (js - zo) + azo
            self.RR[1][2] = (bjs - bzo) * (AA - zo) / (js - zo) + bzo

        elif ((AA >= js) and (AA < zj)):
            self.RR[1][1] = (azj - ajs) * (AA - js) / (zj - js) + ajs
            self.RR[1][2] = (bzj - bjs) * (AA - js) / (zj - js) + bjs
        elif ((AA >= zj) and (AA < ks)):
            self.RR[1][1] = (aks - azj) * (AA - zj) / (ks - zj) + azj
            self.RR[1][2] = (bks - bzj) * (AA - zj) / (ks - zj) + bzj
        elif ((AA >= ks) and (AA < up)):
            self.RR[1][1] = azb
            self.RR[1][2] = bzb

    # 计算传感器俯仰夹角基本可信数
    def GetPAPara_R(self,section,PA,Para=False):
        up=0.
        low=0.
        if Para == True:
            up = math.pi / 2
            low = 0
        else:
            up = math.pi / 2
            low = 0 - math.pi / 2           

            # 变量赋值
        zo = self.find_num(self.Settings[section][1]) * math.pi / 180
        js = self.find_num(self.Settings[section][2]) * math.pi / 180
        zj = self.find_num(self.Settings[section][3]) * math.pi / 180
        ks = self.find_num(self.Settings[section][4]) * math.pi / 180
        zb = self.find_num(self.Settings[section][5]) * math.pi / 180

        o = self.find_num(self.Settings[section][6])

        azo = self.find_num(self.Settings[section][7])
        bzo = self.find_num(self.Settings[section][8])

        ajs = self.find_num(self.Settings[section][9])
        bjs = self.find_num(self.Settings[section][10])

        azj = self.find_num(self.Settings[section][11])
        bzj = self.find_num(self.Settings[section][12])

        aks = self.find_num(self.Settings[section][13])
        bks = self.find_num(self.Settings[section][14])

        azb = self.find_num(self.Settings[section][15])
        bzb = self.find_num(self.Settings[section][16])

        self.RR[2][3] = o

        if ((PA >= low) and (PA < zo)):
            self.RR[2][1] = azo
            self.RR[2][2] = bzo

        elif ((PA >= zo) and (PA < js)):
            self.RR[2][1] = (ajs - azo) * (PA - zo) / (js - zo) + azo
            self.RR[2][2] = (bjs - bzo) * (PA - zo) / (js - zo) + bzo

        elif ((PA >= js) and (PA < zj)):
            self.RR[2][1] = (azj - ajs) * (PA - js) / (zj - js) + ajs
            self.RR[2][2] = (bzj - bjs) * (PA - js) / (zj - js) + bjs

        elif ((PA >= zj) and (PA < ks)):
            self.RR[2][1] = (aks - azj) * (PA - zj) / (ks - zj) + azj
            self.RR[2][2] = (bks - bzj) * (PA - zj) / (ks - zj) + bzj

        elif ((PA >=ks) and (PA < up)):
            self.RR[2][1] = azb
            self.RR[2][2] = bzb

    # 计算武器水平距离基本可信数
    def GetHDPara_M(self,section,HD):
        zo = self.find_num(self.Settings[section][1])
        js = self.find_num(self.Settings[section][2])
        zj = self.find_num(self.Settings[section][3])
        ks = self.find_num(self.Settings[section][4])
        zb = self.find_num(self.Settings[section][5])

        o = self.find_num(self.Settings[section][6])

        azo = self.find_num(self.Settings[section][7])
        bzo = self.find_num(self.Settings[section][8])

        ajs = self.find_num(self.Settings[section][9])
        bjs = self.find_num(self.Settings[section][10])

        azj = self.find_num(self.Settings[section][11])
        bzj = self.find_num(self.Settings[section][12])

        aks = self.find_num(self.Settings[section][13])
        bks = self.find_num(self.Settings[section][14])

        azb = self.find_num(self.Settings[section][15])
        bzb = self.find_num(self.Settings[section][16])

        self.MM[0][3] = o

        if ((HD >= 0) and (HD < zo)):
            self.MM[0][1] = azo
            self.MM[0][2] = bzo
        elif ((HD >= zo) and (HD < js)):
            self.MM[0][1] = (ajs - azo) * (HD - zo) / (js - zo) + azo
            self.MM[0][2] = (bjs - bzo) * (HD - zo) / (js - zo) + bzo
        elif ((HD >= js) and (HD < zj)):
            self.MM[0][1] = (azj - ajs) * (HD - js) / (zj - js) + ajs
            self.MM[0][2] = (bzj - bjs) * (HD - js) / (zj - js) + bjs
        elif ((HD >= zj) and (HD < ks)):
            self.MM[0][1] = (aks - azj) * (HD - zj) / (ks - zj) + azj
            self.MM[0][2] = (bks - bzj) * (HD - zj) / (ks - zj) + bzj
        elif (HD >=ks):
            self.MM[0][1] = azb
            self.MM[0][2] = bzb

    # 计算武器方位夹角基本可信数
    def GetAAPara_M(self,section,AA,Para=True):
        up=0.
        low=0.
        # 空对地
        if (Para == True):
            up = math.pi
            low = 0
        else:
            up = math.pi / 2
            low = 0 - math.pi / 2

            # 变量赋值
        zo = self.find_num(self.Settings[section][1]) * math.pi / 180
        js = self.find_num(self.Settings[section][2]) * math.pi / 180
        zj = self.find_num(self.Settings[section][3]) * math.pi / 180
        ks = self.find_num(self.Settings[section][4]) * math.pi / 180
        zb = self.find_num(self.Settings[section][5]) * math.pi / 180

        o = self.find_num(self.Settings[section][6])

        azo = self.find_num(self.Settings[section][7])
        bzo = self.find_num(self.Settings[section][8])

        ajs = self.find_num(self.Settings[section][9])
        bjs = self.find_num(self.Settings[section][10])

        azj = self.find_num(self.Settings[section][11])
        bzj = self.find_num(self.Settings[section][12])

        aks = self.find_num(self.Settings[section][13])
        bks = self.find_num(self.Settings[section][14])

        azb = self.find_num(self.Settings[section][15])
        bzb = self.find_num(self.Settings[section][16])

        self.MM[1][3] = o

        if (AA < 0):
            AA = 0 - AA

        if ((AA >= low) and (AA < zo)):
            self.MM[1][1] = azo
            self.MM[1][2] = bzo

        elif ((AA >= zo) and (AA < js)):
            self.MM[1][1] = (ajs - azo) * (AA - zo) / (js - zo) + azo
            self.MM[1][2] = (bjs - bzo) * (AA - zo) / (js - zo) + bzo

        elif ((AA >= js) and (AA < zj)):
            self.MM[1][1] = (azj - ajs) * (AA - js) / (zj - js) + ajs
            self.MM[1][2] = (bzj - bjs) * (AA - js) / (zj - js) + bjs

        elif ((AA >= zj) and (AA < ks)):

            self.MM[1][1] = (aks - azj) * (AA - zj) / (ks - zj) + azj
            self.MM[1][2] = (bks - bzj) * (AA - zj) / (ks - zj) + bzj

        elif ((AA >= ks) and (AA < up)):
            self.MM[1][1] = azb
            self.MM[1][2] = bzb

    # 计算武器俯仰夹角基本可信数
    def GetPAPara_M(self,section,PA,Para=False):
        up=0.
        low=0.
        if (Para == True):
            up = math.pi / 2
            low = 0            
        else:
            up = math.pi / 2
            low = 0 - math.pi / 2
            
        # 变量赋值
        zo = self.find_num(self.Settings[section][1]) * math.pi / 180
        js = self.find_num(self.Settings[section][2]) * math.pi / 180
        zj = self.find_num(self.Settings[section][3]) * math.pi / 180
        ks = self.find_num(self.Settings[section][4]) * math.pi / 180
        zb = self.find_num(self.Settings[section][5]) * math.pi / 180

        o = self.find_num(self.Settings[section][6])

        azo = self.find_num(self.Settings[section][7])
        bzo = self.find_num(self.Settings[section][8])

        ajs = self.find_num(self.Settings[section][9])
        bjs = self.find_num(self.Settings[section][10])

        azj = self.find_num(self.Settings[section][11])
        bzj = self.find_num(self.Settings[section][12])

        aks = self.find_num(self.Settings[section][13])
        bks = self.find_num(self.Settings[section][14])

        azb = self.find_num(self.Settings[section][15])
        bzb = self.find_num(self.Settings[section][16])

        self.MM[2][3] = o

        if ((PA >= low) and (PA < zo)):        
            self.MM[2][1] = azo
            self.MM[2][2] = bzo
    
        elif ((PA >= zo) and (PA < js)):
            self.MM[2][1] = (ajs - azo) * (PA - zo) / (js - zo) + azo
            self.MM[2][2] = (bjs - bzo) * (PA - zo) / (js - zo) + bzo

        elif ((PA >= js) and (PA < zj)):
            self.MM[2][1] = (azj - ajs) * (PA - js) / (zj - js) + ajs
            self.MM[2][2] = (bzj - bjs) * (PA - js) / (zj - js) + bjs

        if ((PA >= zj) and (PA < ks)):
            self.MM[2][1] = (aks - azj) * (PA - zj) / (ks - zj) + azj
            self.MM[2][2] = (bks - bzj) * (PA - zj) / (ks - zj) + bzj

        if ((PA >= ks) and (PA < up)):

            self.MM[2][1] = azb
            self.MM[2][2] = bzb

class Cal_Ang_dis:
    def 计算水平距离(self,x1,y1,x2,y2):
        HD=math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2))
        return HD

    def 计算方位夹角_cos(self,x1,y1,x2,y2):
        HD = math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2))
        AA = math.acos((x2 - x1) / HD)
        return AA

    def 计算方位夹角_tan(self,x1,y1,x2,y2):
        return math.atan((x2 - x1) / (y2 - y1))

    def 计算俯仰角(self,x1,y1,z1,x2,y2,z2):
        HD = math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2))
        PA = math.atan((z1 - z2) / HD)
        return PA


